#region [AccelMath Using Statements]
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Diagnostics;
using Microsoft.Xna.Framework;
#endregion

namespace WP7_XNA_Accelerometer_Simulator.Utils.Accelerometer
{
    /// <summary>
    /// AccelMath class supplies mathematical functions useful to calculate the 
    /// acceleration applied to the windows phone 7
    /// </summary>
    public static class AccelMath
    {
        #region [AccelMath Fields]
        private static readonly Vector3 Gravity = new Vector3(0, -1, 0);
        private static readonly Vector3 Ground = new Vector3(1, 0, 1);
        #endregion

        #region [AccelMath Structs]
        /// <summary>
        /// Auxiliary struct used to get the angles between two vectors in a 3D space 
        /// </summary>
        public struct Angles 
        { 
            public float angleXY;
            public float angleXZ; 
            public float angleYZ; 
        }
        #endregion

        #region [AccelMath Public Methods]
        /// <summary>
        /// Calculates the acceleration according to the model's axis normals
        /// </summary>
        /// <param name="rightEdgeNormal">The model's current right edge normal</param>
        /// <param name="topEdgeNormal">The model's current top edge normal</param>
        /// <param name="frontEdgeNormal">The model's current front edge normal</param>
        /// <returns></returns>
        public static Vector3 CalculateAccelValues(Vector3 rightEdgeNormal, Vector3 topEdgeNormal, Vector3 frontEdgeNormal)
        {
            Angles angles;
            Vector3 accelValues;

            angles = FindAnglesBetweenTwoVectors(rightEdgeNormal, Ground);
            accelValues.X = (float)(Math.Abs(Math.Sin(angles.angleXY)) * Math.Sin(angles.angleYZ));
            
            angles = AccelMath.FindAnglesBetweenTwoVectors(topEdgeNormal, Ground);
            accelValues.Y = (float)(Math.Sin(angles.angleXY) * Math.Abs(Math.Sin(angles.angleYZ)));

            angles = AccelMath.FindAnglesBetweenTwoVectors(frontEdgeNormal, Gravity);
            accelValues.Z = (float)(Math.Cos(angles.angleXY) * Math.Abs(Math.Cos(angles.angleYZ)));

            /*
             * Some angles will return NaN value on its Sin or Cos value
             * What we want is to return 0 on such cases
             */
            if (Double.IsNaN(accelValues.X))
                accelValues.X = 0;
            if (Double.IsNaN(accelValues.Y))
                accelValues.Y = 0;
            if (Double.IsNaN(accelValues.Z))
                accelValues.Z = 0;

            return accelValues;
        }
        #endregion

        #region [AccelMath Private/Auxiliary Methods]
        /// <summary> 
        /// Find the XY, XZ and YZ angles between tow vectors 
        /// Order is crucial for angle signal
        /// </summary> 
        /// <param name="vOne">First Vector</param> 
        /// <param name="vTwo">Second vector</param> 
        /// <returns>The XY, XZ and YZ angles calculated</returns> 
        private static Angles FindAnglesBetweenTwoVectors(Vector3 vOne, Vector3 vTwo)
        {
            Angles angle;
            angle.angleXY = FindAngleBetweenTwoVectors(new Vector2(vOne.X, vOne.Y), new Vector2(vTwo.X, vTwo.Y));
            angle.angleXZ = FindAngleBetweenTwoVectors(new Vector2(vOne.X, vOne.Z), new Vector2(vTwo.X, vTwo.Z));
            angle.angleYZ = FindAngleBetweenTwoVectors(new Vector2(vOne.Y, vOne.Z), new Vector2(vTwo.Y, vTwo.Z));
            return angle;
        }

        /// <summary> 
        /// Find the angle between two vectors. The order is crucial for the Signal of angle. 
        /// </summary> 
        private static float FindAngleBetweenTwoVectors(Vector2 vOne, Vector2 vTwo)
        {
            var v1 = new Vector2(vOne.X, vOne.Y);
            var v2 = new Vector2(vTwo.X, vTwo.Y);
            float angle;

            //turn vectors into unit vectors 
            v1.Normalize();
            v2.Normalize();
            float dot = Vector2.Dot(v1, v2);

            if (dot < -1)
                dot = -1;

            angle = (float)Math.Acos(dot);
            
            // if no noticable rotation is available return zero rotation 
            // this way we avoid Cross product artifacts 
            if (Math.Abs(angle) < 0.0001)
                return 0;

            angle *= Signal(v1, v2);

            return angle;
        }

        /// <summary>
        /// Calculates angle signal
        /// </summary>
        /// <param name="v1">First Vector</param>
        /// <param name="v2">Second Vector</param>
        /// <returns>1 or -1</returns>
        private static int Signal(Vector2 v1, Vector2 v2)
        {
            return (v1.Y * v2.X - v2.Y * v1.X) > 0 ? 1 : -1;
        }
        #endregion
    }
}
